﻿#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/core/utils/logger.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/objdetect/aruco_detector.hpp>

#include <filesystem>
#include <vector>
#include <string>
#include <algorithm>
#include <iostream>
#include <iterator>
#include <fstream>
#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>

using namespace cv;
using namespace std;
namespace fs = std::filesystem;

//横向的角点数目
const int boardWidth = 8;
//纵向的角点数目
const int boardHeight = 6;
//总的角点数目
const int boardCorner = boardWidth * boardHeight;
//标定板黑白格子的大小 单位是mm
const int squareSize = 30;
//标定板的总内角点
const Size boardSize = Size(boardWidth, boardHeight);
//Size imageSize = Size(imageWidth, imageHeight);

Size imageSize = Size(0, 0);      //图像的尺寸
Mat R, T, E, F;
//R旋转矢量 T平移矢量 E本征矩阵 F基础矩阵
vector<Mat> rvecs; //R
vector<Mat> tvecs; //T
//左边摄像机所有照片角点的坐标集合
vector<vector<Point2f>> imagePointL;
//右边摄像机所有照片角点的坐标集合
vector<vector<Point2f>> imagePointR;
//各图像的角点的实际的物理坐标集合
vector<vector<Point3f>> objRealPoint;
//左边摄像机某一照片角点坐标集合
vector<Point2f> cornerL;
//右边摄像机某一照片角点坐标集合
vector<Point2f> cornerR;

Mat rgbImageL, grayImageL;
Mat rgbImageR, grayImageR;

Mat intrinsic;
Mat distortion_coeff;

//校正旋转矩阵R，投影矩阵P，重投影矩阵Q
Mat Rl, Rr, Pl, Pr, Q;

// 重投影误差
double rms_l, rms_r, rms_stereo;

//映射表
Mat mapLx, mapLy, mapRx, mapRy;
Rect validROIL, validROIR;

//图像校正之后，会对图像进行裁剪，其中，validROI裁剪之后的区域
/*事先标定好的左相机的内参矩阵
fx 0 cx
0 fy cy
0  0  1
*/
Mat cameraMatrixL;
Mat distCoeffL;
Mat cameraMatrixR;
Mat distCoeffR;

int frameNumber;

// 单目标定，得到相机内参和畸变参数
void single_calibrate(bool left, string path, string result_path)
{
	if (left)
		std::cout << "开始左相机单目标定……" << std::endl;
	else
		std::cout << std::endl << "开始右相机单目标定……" << std::endl;

	// 读取每一幅图像，从中提取出角点，然后对角点进行亚像素精确化
	int image_count = 0;  /* 图像数量 */
	//Size imageSize;      /* 图像的尺寸 */
	//Size boardSize = Size(boardWidth, boardHeight);             /* 标定板上每行、列的角点数 */
	vector<Point2f> image_points_buf;         /* 缓存每幅图像上检测到的角点 */
	vector<vector<Point2f>> image_points_seq; /* 保存检测到的所有角点 */
	string filename;      // 图片名
	vector<string> filenames;

	ofstream fout(result_path);  /* 保存标定结果的文件 */

	std::cout << "	开始读取文件夹图片" << std::endl;
	for (const auto& entry : fs::directory_iterator(path)) {
		std::string filename = entry.path().string();
		if (entry.path().extension() == ".jpg") {
			++image_count;
			Mat imageInput = imread(filename);
			filenames.push_back(filename);

			// 读入第一张图片时获取图片大小
			if (image_count == 1)
			{
				imageSize.width = imageInput.cols;
				imageSize.height = imageInput.rows;
			}

			/* 提取角点 */
			if (0 == findChessboardCorners(imageInput, boardSize, image_points_buf))
			{
				std::cout << "**" << filename << "** 找不到角点!" << std::endl;
				exit(1);
			}
			else
			{
				Mat view_gray;
				cvtColor(imageInput, view_gray, cv::COLOR_BGR2GRAY);  // 转灰度图

				/* 亚像素精确化 */
				// image_points_buf 初始的角点坐标向量，同时作为亚像素坐标位置的输出
				// Size(5,5) 搜索窗口大小
				// （-1，-1）表示没有死区
				// TermCriteria 角点的迭代过程的终止条件, 可以为迭代次数和角点精度两者的组合
				cornerSubPix(view_gray, image_points_buf, Size(5, 5), Size(-1, -1), TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30, 0.1));

				image_points_seq.push_back(image_points_buf);  // 保存亚像素角点

				//在图像上显示角点位置
				//drawChessboardCorners(view_gray, boardSize, image_points_buf, false); // 用于在图片中标记角点
				//imshow("Camera Calibration", view_gray);       // 显示图片

			}
		}
	}

	int CornerNum = boardSize.width * boardSize.height;  // 每张图片上总的角点数

	//-------------以下是摄像机标定------------------

	/*棋盘三维信息*/
	Size square_size = Size(30, 30);         /* 实际测量得到的标定板上每个棋盘格的大小 */
	vector<vector<Point3f>> object_points;   /* 保存标定板上角点的三维坐标 */

	/*内外参数*/
	Mat cameraMatrix = Mat(3, 3, CV_32FC1, Scalar::all(0));  /* 摄像机内参数矩阵 */
	vector<int> point_counts;   // 每幅图像中角点的数量
	Mat distCoeffs = Mat(1, 5, CV_32FC1, Scalar::all(0));       /* 摄像机的5个畸变系数：k1,k2,p1,p2,k3 */
	vector<Mat> tvecsMat;      /* 每幅图像的旋转向量 */
	vector<Mat> rvecsMat;      /* 每幅图像的平移向量 */

	/* 初始化标定板上角点的三维坐标 */
	int i, j, t;
	for (t = 0; t < image_count; t++)
	{
		vector<Point3f> tempPointSet;
		for (i = 0; i < boardSize.height; i++)
		{
			for (j = 0; j < boardSize.width; j++)
			{
				Point3f realPoint;

				/* 假设标定板放在世界坐标系中z=0的平面上 */
				realPoint.x = i * square_size.width;
				realPoint.y = j * square_size.height;
				realPoint.z = 0;
				tempPointSet.push_back(realPoint);
			}
		}
		object_points.push_back(tempPointSet);
	}

	/* 初始化每幅图像中的角点数量，假定每幅图像中都可以看到完整的标定板 */
	for (i = 0; i < image_count; i++)
	{
		point_counts.push_back(boardSize.width * boardSize.height);
	}

	std::cout << "	调用calibrateCamera计算内外参" << std::endl;
	// object_points 世界坐标系中的角点的三维坐标
	// image_points_seq 每一个内角点对应的图像坐标点
	// imageSize 图像的像素尺寸大小
	// cameraMatrix 输出，内参矩阵
	// distCoeffs 输出，畸变系数
	// rvecsMat 输出，旋转向量
	// tvecsMat 输出，位移向量
	// 0 标定时所采用的算法
	double rms = calibrateCamera(object_points, image_points_seq, imageSize, cameraMatrix, distCoeffs, rvecsMat, tvecsMat, 0);

	//std::cout<<" 重投影误差rms：" << rms << std::endl;

	if (left)
	{
		rms_l = rms;
		imagePointL = image_points_seq;
		cameraMatrixL = cameraMatrix;
		distCoeffL = distCoeffs;
	}
	else
	{
		rms_r = rms;
		imagePointR = image_points_seq;
		cameraMatrixR = cameraMatrix;
		distCoeffR = distCoeffs;
	}
	//------------------------标定完成------------------------------------

	// -------------------对标定结果进行评价------------------------------
	std::cout << "	保存结果到文件" << std::endl;

	fout << "重投影误差rms：" << rms << std::endl << std::endl;
	double total_err = 0.0;         /* 所有图像的平均误差的总和 */
	double err = 0.0;               /* 每幅图像的平均误差 */
	vector<Point2f> image_points2;  /* 保存重新计算得到的投影点 */
	fout << "每幅图像的标定误差：\n";

	for (i = 0; i < image_count; i++)
	{
		vector<Point3f> tempPointSet = object_points[i];

		/* 通过得到的摄像机内外参数，对空间的三维点进行重新投影计算，得到新的投影点 */
		projectPoints(tempPointSet, rvecsMat[i], tvecsMat[i], cameraMatrix, distCoeffs, image_points2);

		/* 计算新的投影点和旧的投影点之间的误差*/
		vector<Point2f> tempImagePoint = image_points_seq[i];
		Mat tempImagePointMat = Mat(1, tempImagePoint.size(), CV_32FC2);
		Mat image_points2Mat = Mat(1, image_points2.size(), CV_32FC2);

		for (int j = 0; j < tempImagePoint.size(); j++)
		{
			image_points2Mat.at<Vec2f>(0, j) = Vec2f(image_points2[j].x, image_points2[j].y);
			tempImagePointMat.at<Vec2f>(0, j) = Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);
		}
		err = norm(image_points2Mat, tempImagePointMat, NORM_L2);
		total_err += err /= point_counts[i];
		fout << "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << std::endl;
	}
	fout << "总体平均误差：" << total_err / image_count << "像素" << std::endl << std::endl;

	//-------------------------评价完成---------------------------------------------

	//-----------------------保存定标结果到文件------------------------------------------- 
	Mat rotation_matrix = Mat(3, 3, CV_32FC1, Scalar::all(0));  /* 保存每幅图像的旋转矩阵 */
	fout << "相机内参数矩阵：" << std::endl;
	fout << cameraMatrix << std::endl << std::endl;
	fout << "畸变系数：\n";
	fout << distCoeffs << std::endl << std::endl << std::endl;

	for (int i = 0; i < image_count; i++)
	{
		fout << "第" << i + 1 << "幅图像的旋转向量：" << std::endl;
		fout << tvecsMat[i] << std::endl;

		/* 将旋转向量转换为相对应的旋转矩阵 */
		Rodrigues(tvecsMat[i], rotation_matrix);
		fout << "第" << i + 1 << "幅图像的旋转矩阵：" << std::endl;
		fout << rotation_matrix << std::endl;
		fout << "第" << i + 1 << "幅图像的平移向量：" << std::endl;
		fout << rvecsMat[i] << std::endl << std::endl;
	}
	fout << std::endl;
	std::cout << "	【成功！】写入单目标定结果到文件：" << result_path << std::endl;

	//--------------------标定结果保存结束-------------------------------

	//----------------------显示定标结果--------------------------------

	Mat mapx = Mat(imageSize, CV_32FC1);
	Mat mapy = Mat(imageSize, CV_32FC1);
	Mat R = Mat::eye(3, 3, CV_32F);
	string imageFileName;
	std::stringstream StrStm;
	for (int i = 0; i != image_count; i++)
	{
		initUndistortRectifyMap(cameraMatrix, distCoeffs, R, cameraMatrix, imageSize, CV_32FC1, mapx, mapy);
		Mat imageSource = imread(filenames[i]);
		Mat newimage = imageSource.clone();
		remap(imageSource, newimage, mapx, mapy, INTER_LINEAR);
		StrStm.clear();
		imageFileName.clear();
		StrStm << i + 1;
		StrStm >> imageFileName;
		//imageFileName += "_d.jpg";
		//imwrite(imageFileName, newimage);
	}
	frameNumber = image_count;
	fout.close();
}

// 计算标定板上模块的实际物理坐标
void calRealPoint(vector<vector<Point3f>>& obj, int boardWidth, int boardHeight, int imgNumber, int squareSize)
{
	vector<Point3f> imgpoint;
	for (int rowIndex = 0; rowIndex < boardHeight; rowIndex++)
	{
		for (int colIndex = 0; colIndex < boardWidth; colIndex++)
		{
			imgpoint.push_back(Point3f(rowIndex * squareSize, colIndex * squareSize, 0));
		}
	}
	for (int imgIndex = 0; imgIndex < imgNumber; imgIndex++)
	{
		obj.push_back(imgpoint);
	}
}

// 输出参数
void outputCameraParam(string path)
{
	FileStorage fs(path + "intrisics.yml", FileStorage::WRITE);
	if (fs.isOpened())
	{
		fs << "cameraMatrixL" << cameraMatrixL << "cameraDistcoeffL" << distCoeffL << "cameraMatrixR" << cameraMatrixR << "cameraDistcoeffR" << distCoeffR;
		fs.release();
		//std::cout << "写入双目标定结果（内参，畸变）到文件：" << path + "intrisics.yml";
		//std::cout << "cameraMatrixL=:" << cameraMatrixL << std::endl << "cameraDistcoeffL=:" << distCoeffL << std::endl << "cameraMatrixR=:" << cameraMatrixR << std::endl << "cameraDistcoeffR=:" << distCoeffR << std::endl;
	}
	else
	{
		std::cout << "Error: can not save the intrinsics!!!!" << std::endl;
	}

	fs.open(path + "extrinsics.yml", FileStorage::WRITE);
	if (fs.isOpened())
	{
		fs << "R" << R << "T" << T << "Rl" << Rl << "Rr" << Rr << "Pl" << Pl << "Pr" << Pr << "Q" << Q;
		//std::cout << "R=" << R << std::endl << "T=" << T << std::endl << "Rl=" << Rl << std::endl << "Rr" << Rr << std::endl << "Pl" << Pl << std::endl << "Pr" << Pr << std::endl << "Q" << Q << std::endl;
		fs.release();
		//std::cout << "写入双目标定结果（R、T、……）到文件：" << path + "extrinsics.yml";
	}
	else
	{
		std::cout << "Error: can not save the extrinsic parameters\n";
	}

	fs.open(path + "rms_stereo.txt", FileStorage::WRITE);
	if (fs.isOpened())
	{
		fs << "rms_stereo" << rms_stereo;
		fs.release();
		//std::cout << "写入双目标定结果（rms_stereo）到文件：" << path + "rms_stereo.txt";
	}
	else
	{
		std::cout << "Error: can not save the rms_stereo\n";
	}
}

// 双目标定
void stereo_calibrate(String path)
{
	// 路径，最后的斜杠要加
	std::string left_path = path + "left/";
	std::string right_path = path + "right/";

	Mat img;
	int goodFrameCount = 0;

	// 先单目标定
	single_calibrate(true, left_path, path + "caliberation_result_left.txt");
	single_calibrate(false, right_path, path + "caliberation_result_right.txt");

	//计算实际的校正点的三维坐标，根据实际标定格子的大小来设置
	calRealPoint(objRealPoint, boardWidth, boardHeight, frameNumber, squareSize);

	//标定摄像头
	std::cout << std::endl << "开始双目标定……" << std::endl;

	rms_stereo = stereoCalibrate(objRealPoint, imagePointL, imagePointR,
		cameraMatrixL, distCoeffL,
		cameraMatrixR, distCoeffR,
		imageSize, R, T, E, F, CALIB_USE_INTRINSIC_GUESS,
		TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 100, 1e-5));

	std::cout << "双目重投影误差rms：" << rms_stereo << std::endl;

	stereoRectify(cameraMatrixL, distCoeffL, cameraMatrixR, distCoeffR, imageSize, R, T, Rl,
		Rr, Pl, Pr, Q, CALIB_ZERO_DISPARITY, -1, imageSize, &validROIL, &validROIR);

	initUndistortRectifyMap(cameraMatrixL, distCoeffL, Rl, Pl, imageSize, CV_32FC1, mapLx, mapLy);
	initUndistortRectifyMap(cameraMatrixR, distCoeffR, Rr, Pr, imageSize, CV_32FC1, mapRx, mapRy);

	// 标定结果写入文件
	outputCameraParam(path);
}

// 拼接两张图像，用于显示校正前后效果
Mat concatImage(Mat imgL, Mat imgR)
{
	Mat canvas;
	double sf;
	int w, h;
	sf = 600. / MAX(imageSize.width, imageSize.height);
	w = cvRound(imageSize.width * sf);
	h = cvRound(imageSize.height * sf);
	canvas.create(h, w * 2, CV_8UC3);

	//左图像画到画布上
	Mat canvasPart = canvas(Rect(0, 0, w, h));
	resize(imgL, canvasPart, canvasPart.size(), 0, 0, INTER_AREA);
	Rect vroiL(cvRound(validROIL.x * sf), cvRound(validROIL.y * sf),
		cvRound(validROIL.width * sf), cvRound(validROIL.height * sf));
	rectangle(canvasPart, vroiL, Scalar(0, 0, 255), 2, 8);


	//右图像画到画布上
	canvasPart = canvas(Rect(w, 0, w, h));
	resize(imgR, canvasPart, canvasPart.size(), 0, 0, INTER_LINEAR);
	Rect vroiR(cvRound(validROIR.x * sf), cvRound(validROIR.y * sf),
		cvRound(validROIR.width * sf), cvRound(validROIR.height * sf));
	rectangle(canvasPart, vroiR, Scalar(0, 255, 255), 2, 8);

	//画上对应的线条
	for (int i = 0; i < canvas.rows; i += 16)
		line(canvas, Point(0, i), Point(canvas.cols, i), Scalar(0, 255, 0), 1, 8);

	return canvas;
}

// 基线校正
void rectify(String path)
{
	std::string left_path = path + "left/";
	std::string right_path = path + "right/";
	std::string rectified_left_path = path + "rectified_left/";
	std::string rectified_right_path = path + "rectified_right/";
	std::string rectified_compare_before_path = path + "rectified_compare_before/";
	std::string rectified_compare_after_path = path + "rectified_compare_after/";

	if (!fs::exists(rectified_left_path))
		fs::create_directories(rectified_left_path);
	if (!fs::exists(rectified_right_path))
		fs::create_directories(rectified_right_path);
	if (!fs::exists(rectified_compare_before_path))
		fs::create_directories(rectified_compare_before_path);
	if (!fs::exists(rectified_compare_after_path))
		fs::create_directories(rectified_compare_after_path);

	std::cout << std::endl << "读取文件夹每幅图片进行校正" << std::endl;

	for (const auto& entry : fs::directory_iterator(left_path)) {
		std::string filename = entry.path().string();
		if (entry.path().extension() == ".jpg") {
			std::string left_filename = entry.path().string();
			std::string right_filename = right_path + entry.path().filename().string();

			rgbImageL = imread(left_filename, cv::IMREAD_COLOR);
			rgbImageR = imread(right_filename, cv::IMREAD_COLOR);

			//经过remap之后，左右相机的图像已经共面并且行对准了
			Mat rectifyImageL, rectifyImageR;
			remap(rgbImageL, rectifyImageL, mapLx, mapLy, INTER_LINEAR);
			remap(rgbImageR, rectifyImageR, mapRx, mapRy, INTER_LINEAR);

			cv::imwrite(rectified_left_path + entry.path().filename().string(), rectifyImageL);
			cv::imwrite(rectified_right_path + entry.path().filename().string(), rectifyImageR);

			//保存校正前后结果
			cv::imwrite(rectified_compare_before_path + entry.path().filename().string(), concatImage(rgbImageL, rgbImageR));
			cv::imwrite(rectified_compare_after_path + entry.path().filename().string(), concatImage(rectifyImageL,rectifyImageR));
		}
	}
	std::cout << "校正结束" << std::endl;
	std::cout << "左校正图片路径：" + rectified_left_path << std::endl;
	std::cout << "右校正图片路径：" + rectified_right_path << std::endl;
	std::cout << "左右校正前图片拼接路径：" + rectified_compare_before_path << std::endl;
	std::cout << "左右校正后图片拼接路径：" + rectified_compare_after_path << std::endl;
}

// 检测aruco标记并返回它们的中心坐标，传出带有标记的image
cv::Point2f detectArucoMarker(cv::Mat& image)
{
	cv::aruco::DetectorParameters detectorParams = cv::aruco::DetectorParameters();
	cv::aruco::Dictionary dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);
	cv::aruco::ArucoDetector detector(dictionary, detectorParams);

	std::vector<int> ids;
	std::vector<std::vector<cv::Point2f>> corners;
	detector.detectMarkers(image, corners, ids);

	if (corners.empty()) {
		throw std::runtime_error("No aruco marker detected");
	}

	cv::aruco::drawDetectedMarkers(image, corners, ids);

	cv::Point2f center = (corners[0][0] + corners[0][2]) / 2.0;
	//cout << center << endl;
	//cv::imwrite("image.jpg", image);
	return center;
}

// 将文件名和中心坐标写入CSV文件
void writeToFile(const std::string& filename, const std::string& imageFilename,
	const cv::Point2f& centerL, const cv::Point2f& centerR)
{
	std::ofstream file(filename, std::ios::app);
	// 移动写指针到文件末尾，以确定文件的大小
	file.seekp(0, std::ios::end);

	// 检查文件是否为空，如果是则写入标题行
	if (file.tellp() == 0) {
		file << "imageFilename,centerL_x,centerL_y,centerR_x,centerR_y,delta_x,delta_y\n";
	}
	file << imageFilename << "," << centerL.x << "," << centerL.y << "," << centerR.x << "," << centerR.y << "," << centerL.x - centerR.x << "," << centerL.y - centerR.y << "\n";
}

// 跟踪aruco标记
void track_aruco(string path)
{
	string result_path = path + "track/";
	string left_path = path + "rectified_left/";
	string right_path = path + "rectified_right/";

	if (fs::exists(result_path))
		fs::remove_all(result_path);
	if (!fs::exists(result_path + "left/"))
		fs::create_directories(result_path + "left/");
	if (!fs::exists(result_path + "right/"))
		fs::create_directories(result_path + "right/");

	for (const auto& entry : fs::directory_iterator(left_path)) {
		std::string filename = entry.path().string();
		if (entry.path().extension() == ".jpg") {
			std::string left_filename = entry.path().string();
			std::string right_filename = right_path + entry.path().filename().string();

			// 检测左图marker
			cv::Mat imageL = cv::imread(left_filename);
			cv::Point2f centerL = detectArucoMarker(imageL);

			// 如果检测不到marker，跳过
			if (centerL.x == 0 && centerL.y == 0) {
				std::cout << left_filename << "文件中未检测到aruco marker" << std::endl;
				continue;
			}

			cv::imwrite(result_path + "left/" + entry.path().filename().string(), imageL);

			// 检测右图marker
			cv::Mat imageR = cv::imread(right_filename);
			cv::Point2f centerR = detectArucoMarker(imageR);
			// 如果检测不到marker，跳过
			if (centerR.x == 0 && centerR.y == 0) {
				std::cout << right_filename << "文件中未检测到aruco marker" << std::endl;
				continue;
			}

			cv::imwrite(result_path + "right/" + entry.path().filename().string(), imageR);

			// 中心点信息写入文件
			writeToFile(result_path + "result.csv", entry.path().filename().string(), centerL, centerR);
		}
	}
}

// 主函数
int main()
{
	cv::utils::logging::setLogLevel(utils::logging::LOG_LEVEL_SILENT);
#if 1
	// 标定文件夹路径
	string biaoding_path = "D:/Users/XiangyeHuang/Pictures/xyz_platform_datas/biaoding/";
	stereo_calibrate(biaoding_path);	// 单目、双目标定

	// 基线校正文件夹路径
	vector<string> rectify_paths = { "D:/Users/XiangyeHuang/Pictures/xyz_platform_datas/biaoding/" ,
	"D:/Users/XiangyeHuang/Pictures/xyz_platform_datas/x0.5cm/",
	"D:/Users/XiangyeHuang/Pictures/xyz_platform_datas/y0.5cm/",
	"D:/Users/XiangyeHuang/Pictures/xyz_platform_datas/z0.5cm/"
	};

	// 基线校正+目标跟踪
	for (int i = 0; i < rectify_paths.size(); ++i)
	{
		rectify(rectify_paths[i]);
		if (i > 0)
			track_aruco(rectify_paths[i]);
	}
#endif
	return 0;
}